#include "robot.h"

int main(void)
{
    frame *arelu;
    frame *arelb;
    frame *urelc;
    frame *crelb;
    frame *inv_arelu;
    frame *inv_urelc;
    frame *tmp;
    vec3 v1={11.0,-1.0,30.0};
    vec3 v2={0.0, 7.0, 45.0};
    vec3 v3={-3.0, -3.0, -30.0};
    vec3 out;
    int i=0;
       
    arelu = UTOI(v1);
    arelb = UTOI(v2);
    urelc = UTOI(v3);
    
    inv_arelu = TINVERT(arelu);
    /* printf("Imprimiendo inv_arelu en forma matricial:\n"); */
    /* gsl_matrix_fprintf(stdout, inv_arelu, "%f"); */
    inv_urelc = TINVERT(urelc);
    /* printf("Imprimiendo inv_urelc en forma matricial:\n"); */
    /* gsl_matrix_fprintf(stdout, inv_urelc, "%f"); */
    tmp = TMULT(arelb, inv_arelu);
    crelb = TMULT(tmp, inv_urelc);
    ITOU(crelb, out);
    
    /* Imprimimos crelb en forma de vector */
    printf("Imprimiendo crelb en forma de vector:\n");
    for (i=0; i<3; i++)
        printf("out[%i]=%f\t", i+1, out[i]);
    printf("\n\n");
    
    /* Ahora en forma matricial */
    printf("Imprimiendo crelb en forma matricial:\n");
    gsl_matrix_fprintf(stdout, crelb, "%f");
    
    /* Liberamos la memoria que asignamos a cada matriz */
    gsl_matrix_free(arelu);
    arelu = NULL;
    gsl_matrix_free(arelb);
    arelb = NULL;
    gsl_matrix_free(urelc);
    urelc = NULL;
    gsl_matrix_free(crelb);
    crelb = NULL;
    gsl_matrix_free(inv_arelu);
    inv_arelu = NULL;
    gsl_matrix_free(tmp);
    tmp = NULL;
    return 0;
}
        
